#include "associate_factor.h"
#include "../global.h"
#include "../tracker/tracker.h"
#include "../utils/utils.h"

#include <assert.h>
#include <cmath>
#include <set>

namespace fusion_perception {

AssociateFactor::AssociateFactor(std::string name, int32_t weight)
    : name_(std::move(name))
    , weight_(weight)
{
}

std::string AssociateFactor::getName(void) const
{
    return name_;
}

int32_t AssociateFactor::getWeight(void) const
{
    return weight_;
}

int32_t AssociateFactor::assessScore(const Object&, const Object&) const
{
    return 0;
}

int32_t AssociateFactor::assessScore(std::shared_ptr<Tracker> tracker, const Object& o2) const
{
    return assessScore(*tracker->get(), o2);
}

int32_t FeatureAssociateFactor::assessScore(const Object& o1, const Object& o2) const
{
    if (o1.extend.feature.empty() || o2.extend.feature.empty()) {
        return 0;
    }

    // 定义原始区间的范围
    const double originalMin = -1.0;
    const double originalMax = 0.3;
    // 定义新区间的范围
    const double newMin = 100.0;
    const double newMax = 0.0;
    // 计算斜率 m
    const double m = (newMax - newMin) / (originalMax - originalMin);
    // 计算截距 b
    const double b = newMax - m * originalMax;

    auto ret = cosineDist(o1.extend.feature, o2.extend.feature);

    if (ret > originalMax || ret < originalMin) {
        return -1;
    }

    return m * ret + b;
}

int32_t PositionXAssociateFactor::assessScore(const Object& o1, const Object& o2) const
{
    auto dxThreshold = Global::getInstance().dxThreshold;

    auto dt = (o1.header.stamp.sec - o2.header.stamp.sec) + ((double)o1.header.stamp.nanosec - o2.header.stamp.nanosec) / 1000000000;

    auto x = o1.x + dt * o1.speed * cos(o1.heading * M_PI / 180);
    auto dx = std::abs(x - o2.x);

    if (dx > dxThreshold) {
        return -1;
    }

    return (1.0 - dx / dxThreshold) * 100;
}

int32_t PositionYAssociateFactor::assessScore(const Object& o1, const Object& o2) const
{
    auto dyThreshold = Global::getInstance().dyThreshold;

    auto dt = (o1.header.stamp.sec - o2.header.stamp.sec) + ((double)o1.header.stamp.nanosec - o2.header.stamp.nanosec) / 1000000000;

    auto y = o1.y + dt * o1.speed * sin(o1.heading * M_PI / 180);

    auto dy = std::abs(y - o2.y);

    if (dy > dyThreshold) {
        return -1;
    }

    return (1.0 - dy / dyThreshold) * 100;
}

int32_t HeadingAssociateFactor::assessScore(const Object& o1, const Object& o2) const
{
    auto headingThreshold = Global::getInstance().headingThreshold;

    auto headingDiff = o1.heading - o2.heading;

    if (abs(headingDiff) > headingThreshold) {
        return -1;
    }

    return (1.0 - abs(headingDiff) / headingThreshold) * 100;
}

int32_t TypeAssociateFactor::assessScore(const Object& o1, const Object& o2) const
{
    static std::vector<std::set<int32_t>> types {
        { 104, 105 }, // 静止的
        { 3, 101, 102, 103 }, // 移动的非机动车
        { 1, 2, 4, 5, 6 } // 移动的机动车
    };

    int32_t o1TypeClass = -1;
    int32_t o2TypeClass = -1;

    for (size_t i = 0; i < types.size(); i++) {
        if (types[i].find(o1.type) != types[i].end()) {
            o1TypeClass = i;
        }
        if (types[i].find(o2.type) != types[i].end()) {
            o2TypeClass = i;
        }

        if (o1TypeClass != -1 && o2TypeClass != -1) {
            break;
        }
    }

    if (o1TypeClass == -1 || o2TypeClass == -1) {
        return 0;
    }

    if (o1TypeClass == o2TypeClass) {
        return 100;
    }

    return 0;
}

int32_t HistoryIdAssociateFactor::assessScore(std::shared_ptr<Tracker> tracker, const Object& o2) const
{
    size_t loopIndex = 0;
    size_t refCounter = 0;
    const size_t loopMaxNum = 10;

    auto history = tracker->getHistory();

    for (auto& i : history) {
        if (loopIndex == loopMaxNum) {
            break;
        }

        for (auto& j : std::get<1>(i)) {
            if (j->id == o2.id && j->header.frame_id == o2.header.frame_id) {
                refCounter++;
            }
        }

        for (auto& j : std::get<2>(i)) {
            if (j->id == o2.id && j->header.frame_id == o2.header.frame_id) {
                refCounter++;
            }
        }

        loopIndex++;
    }

    if (refCounter >= size_t(0.3 * loopMaxNum)) {
        return 100;
    } else if (refCounter >= size_t(0.2 * loopMaxNum)) {
        return 80;
    } else if (refCounter >= size_t(0.1 * loopMaxNum)) {
        return 70;
    } else if (refCounter >= 1) {
        return 60;
    } else {
        return 0;
    }

    return 0;
}

}